#include "linear2d2s_mpc.h"
namespace auto_ros
{
namespace control
{
//c
Linear2d2sMpc::Linear2d2sMpc(const std::string dyn_config_filename, const std::string mpc_config_filename, double rate)
	: rate_(rate)
{

	YAML::Node dyn_yaml = YAML::LoadFile(dyn_config_filename);
	mpc_yaml_ = YAML::LoadFile(mpc_config_filename);
	//(double mass, double cf, double cr, double lf, double lr, double iz,
	//				  double max_lateral_acc = 9, double max_tire_f_angle = 0.5);
	vehicle_dyn_ptr_ = std::make_shared<VehicleLinear2d2s>(
		dyn_yaml["mass"].as<double>(), dyn_yaml["cf"].as<double>(), dyn_yaml["cr"].as<double>(),
		dyn_yaml["lf"].as<double>(), dyn_yaml["lr"].as<double>(), dyn_yaml["iz"].as<double>(),
		dyn_yaml["max_lateral_acc"].as<double>(), dyn_yaml["max_tire_f_angle"].as<double>());

	dis_dyn_ptr_ = vehicle_dyn_ptr_->discretization(1 / rate_);
	//dis_dyn_ptr, mpc_windows, max_xu_constraint, max_delta_x, max_delta_u, max_final_constraint
	vehicle_mpc_ptr_ = std::make_shared<Mpc>(dis_dyn_ptr_, mpc_yaml_["mpc_windows"].as<int>(),
											 1, 0, 1, 0);
	/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
						   const std::vector<double> u_index, const std::vector<double> u_factor,
						   int constraint_index, double min, double max)*/
	//init constriants or add add static constriants
	vehicle_mpc_ptr_->set_xu_affine({}, {}, {0}, {1}, 0,
									-vehicle_dyn_ptr_->max_tire_f_angle(), vehicle_dyn_ptr_->max_tire_f_angle());
	vehicle_mpc_ptr_->set_delta_u(0, 0, -mpc_yaml_["max_steering_rate"].as<double>() / rate_,
								  mpc_yaml_["max_steering_rate"].as<double>() / rate_);

} // namespace controlLinear2d2sMpc::Linear2d2sMpc(conststd::stringdyn_config_filename,conststd::stringmpc_config_filename,doublerate):rate_(rate)
bool Linear2d2sMpc::step(const Eigen::MatrixXd &q, const Eigen::MatrixXd &r,
						 const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb, const Eigen::MatrixXd &x_ref,
						 const Eigen::MatrixXd &distur_ref, double speed)
{
	vehicle_dyn_ptr_->speed_update_matrix(speed);
	//std::cout << "base_state_matrix is:@@@@@@@@@@@@@" << vehicle_dyn_ptr_->base_state_matrix() << std::endl;
	//std::cout << "state_matrix is:@@@@@@@@@@@@@" << vehicle_dyn_ptr_->state_matrix() << std::endl;
	vehicle_dyn_ptr_->discrete(dis_dyn_ptr_, 1 / rate_, "tustin");
	//add dynamic update constriants below if needed
	//int Mpc::set_xu_bound(int constraint_index, double min, double max)
	double max_angle = vehicle_dyn_ptr_->speed_update_max_tire_f_angle(speed);
	std::cout << "#################### max angle is:" << max_angle << std::endl;
	vehicle_mpc_ptr_->set_xu_bound(0, -max_angle, max_angle);

	//
	/*bool Mpc::update_mpc(
		const Eigen::MatrixXd &q, const Eigen::MatrixXd &r,
		const Eigen::VectorXd &x0, const Eigen::MatrixXd &x_ref,
		const Eigen::MatrixXd &distur_ref) // dis_sys_ptr is  time invarinat*/
	if (!vehicle_mpc_ptr_->update_mpc(q, r, x0, u_fb, x_ref, distur_ref))
	{
		std::cout << "\033[31m Error:Linear2d2sMpc::step failed,vehicle_mpc_ptr_->update_mpc. \033[0m" << std::endl;
		return false;
	}
	if (!(vehicle_mpc_ptr_->mpc_solve()))
	{
		std::cout << "\033[31m Error:Linear2d2sMpc::step failed,vehicle_mpc_ptr_->mpc_solve(). \033[0m" << std::endl;
		return false;
	}
	return true;
}
bool Linear2d2sMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb, const Eigen::MatrixXd &x_ref,
						 const Eigen::MatrixXd &distur_ref, double speed)
{
	Eigen::MatrixXd q_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->state_dim(), vehicle_dyn_ptr_->state_dim());
	q_temp(0, 0) = mpc_yaml_["lateral_error_factor"].as<double>();
	q_temp(1, 1) = mpc_yaml_["lateral_error_rate_factor"].as<double>();
	q_temp(2, 2) = mpc_yaml_["heading_error_factor"].as<double>();
	q_temp(3, 3) = mpc_yaml_["heading_error_rate_factor"].as<double>();
	Eigen::MatrixXd r_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->control_dim(), vehicle_dyn_ptr_->control_dim());
	std::cout << "$$$$$$$$$$$$$$$$$$$$" << std::endl;
	std::cout << q_temp << std::endl;
	r_temp(0, 0) = mpc_yaml_["f_tire_factor"].as<double>();
	if (!step(q_temp, r_temp, x0, u_fb, x_ref, distur_ref, speed))
	{
		return false;
	}
	return true;
}
YAML::Node Linear2d2sMpc::mpc_yaml()
{
	return mpc_yaml_;
}

} // namespace control
} // namespace auto_ros